/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/

#pragma warning(disable:4244)	
#pragma warning(disable:4267)
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif


#include "ViconVFileData.h"
#include "ViconSubject.h"

#include <assert.h>
#include <algorithm>	
#include <math.h>
#include <limits>

#ifndef MAX
#define MAX(a,b) a > b ? a : b
#endif


// reads any type from a filestream
template< typename T >
void readType(std::ifstream &v, T *foo) 
{
	v.read((char *)foo, sizeof(T));
	if (v.fail()) 
	{
		std::cout << "ERROR: read failed" << std::endl;
	}
};


#ifdef __ppc__
/* Current architecture is PowerPC
 * The following overriding functions are required to convert Vicon's V
 * words from little to big endian (so that they work on PowerPC)
 */

inline void littleToBig64(unsigned char * data)
{
		for (char i = 0; i < 4; i++) {
			unsigned char t = data[i];
			data[i] = data[7-i];
			data[7-i] = t;
		}
}

inline void littleToBig32(unsigned char * data)
{
	unsigned char t = data[0];
	data[0] = data[3];
	data[3] = t;
	
	t = data[1];
	data[1] = data[2];
	data[2] = t;
}

inline void littleToBig16(unsigned char * data)
{
	unsigned char t = data[0];
	data[0] = data[1];
	data[1] = t;
};

// unsigned long (32-bit)
void readType(std::ifstream &v, unsigned int * x)
{
	v.read((char *) x, sizeof(unsigned int));
	if (v.fail())
	{
		std::cout << "ERROR: unsigned int read failed" << std::endl;
	}
	littleToBig32((unsigned char * ) x);
};

// signed long (32-bit)
void readType(std::ifstream &v, int * x)
{
	v.read((char *) x, sizeof(int));
	if (v.fail())
	{
		std::cout << "ERROR: int read failed" << std::endl;
	}
	littleToBig32((unsigned char *) x);
};

// signed short (16-bit)
void readType(std::ifstream &v, short * x)
{
	v.read((char *) x, sizeof(short));
	if (v.fail())
	{
		std::cout << "ERROR: short read failed" << std::endl;
	}
	littleToBig16((unsigned char *) x);
}

// unsigend short (16-bit)
void readType(std::ifstream &v, unsigned short * x)
{
	v.read((char *) x, sizeof(unsigned short));
	if (v.fail())
	{
		std::cout << "ERROR: unsigned short read failed" << std::endl;
	}
	littleToBig16((unsigned char *) x);
}

// float (32-bit)
void readType(std::ifstream &v, float * x)
{
	v.read((char *) x, sizeof(float));
	if (v.fail())
	{
		std::cout << "ERROR: float read failed" << std::endl;
	}
	littleToBig32((unsigned char *) x);
}

// double (64-bit)
void readType(std::ifstream &v, double * x)
{
	v.read((char *) x, sizeof(double));
	if (v.fail())
	{
		std::cout << "ERROR: double read failed" << std::endl;
	}
	littleToBig64((unsigned char *) x);
}

#endif


ViconVFileData::ViconVFileData()
{
	m_tType = MT_VFILE;
	m_fCurrTime = 0.0f;
	m_fSpeed = 1.0f;
	m_iTotFrames = 0;

	m_iMarkerGroupID = -1;
	m_iLocalBodyGroupID = -1;
	m_iGlobalBodyGroupID = -1;

	m_iNumMarkerChannels = 0;
	m_iNumGBodyChannels  = 0;
	m_iNumLBodyChannels  = 0;

}

ViconVFileData::~ViconVFileData()
{
	// clear all data storage
	m_vMarkerData.clear();
	m_vBodyData.clear();

}

void ViconVFileData::rewind(void)
{
	m_fCurrTime = 0.0f;
	m_iCurrFrame = 0;
}

void ViconVFileData::update(float addedTime)
{
	// no interpolation for now
	m_fCurrTime += (addedTime*m_fSpeed);
	
	if (m_fCurrTime < 0.0f) {
		m_iCurrFrame = 0;
	}
	else
	{
		m_iCurrFrame = m_fCurrTime * m_fFrameRate;
		if (m_iCurrFrame > m_iTotFrames -1)
		{
			m_iCurrFrame = 0;
			m_fCurrTime = 0.0f;
		}
	}

	if (m_bDebug)
		updateDebug();
}

void ViconVFileData::updateDebug()
{
	// update debug body info
	m_sDebugBodyInfo = "";
	char istr[256];
	std::string label;
	label  = "=========================================================================\n";
	label += " Body Name     GLOBAL:  xpos      ypos      zpos     xrot   yrot   zrot\n";
	label += "=========================================================================\n";
	m_sDebugBodyInfo += label;
	int numBody = m_vBodyChannels.size();
	for (int i=0; i<numBody; i++)
	{
		sprintf(istr, "%-20s %8.2f  %8.2f  %8.2f   %6.1f  %6.1f  %6.1f \n",	
					 m_vBodyChannels[i].Name.c_str(),
					 m_vBodyData[m_iCurrFrame][i].TX / 10.0f,
					 m_vBodyData[m_iCurrFrame][i].TY / 10.0f,
					 m_vBodyData[m_iCurrFrame][i].TZ / 10.0f,
					 Radian(m_vBodyData[m_iCurrFrame][i].eulerX).valueDegrees(),
					 Radian(m_vBodyData[m_iCurrFrame][i].eulerY).valueDegrees(),
					 Radian(m_vBodyData[m_iCurrFrame][i].eulerZ).valueDegrees());

		m_sDebugBodyInfo += istr;
	}
	
	// update debug marker info

}

void ViconVFileData::getMarkerPosition(int id, float* pos)
{
	// P-X, P-Y, P-Z
	if (id < 0 || id > m_vMarkerChannels.size() - 1)
	{
		pos[0] = pos[1] = pos[2] = 0.0f;
		return;
	}

	// unit is mm. so divide it by 10 to make cm scale
	pos[0] = m_vMarkerData[m_iCurrFrame][id].X / 10.0f;
	pos[1] = m_vMarkerData[m_iCurrFrame][id].Y / 10.0f;
	pos[2] = m_vMarkerData[m_iCurrFrame][id].Z / 10.0f;

}

void ViconVFileData::getBodyTranslation(int id, float* trans, bool global)
{
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		trans[0] = trans[1] = trans[2] = 0.0f;
		return;
	}

	// T-X, T-Y, T-Z : t-X, t-Y, t-Z
	if (global)
	{
		trans[0] = m_vBodyData[m_iCurrFrame][id].TX / 10.0f;
		trans[1] = m_vBodyData[m_iCurrFrame][id].TY / 10.0f;
		trans[2] = m_vBodyData[m_iCurrFrame][id].TZ / 10.0f;
	}
	else
	{
		trans[0] = m_vBodyData[m_iCurrFrame][id].tX / 10.0f;
		trans[1] = m_vBodyData[m_iCurrFrame][id].tY / 10.0f;
		trans[2] = m_vBodyData[m_iCurrFrame][id].tZ / 10.0f;
	}
}

void ViconVFileData::getBodyRotation(int id, float* rot, bool global)
{
	// A-X, A-Y. A-Z : a-X, a-Y, a-Z
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		rot[0] = rot[1] = rot[2] = 0.0f;
		return;
	}

	if (global)
	{
		rot[0] = m_vBodyData[m_iCurrFrame][id].EulerX;
		rot[1] = m_vBodyData[m_iCurrFrame][id].EulerY;
		rot[2] = m_vBodyData[m_iCurrFrame][id].EulerZ;
	}
	else
	{
		rot[0] = m_vBodyData[m_iCurrFrame][id].eulerX;
		rot[1] = m_vBodyData[m_iCurrFrame][id].eulerY;
		rot[2] = m_vBodyData[m_iCurrFrame][id].eulerZ;

		m_pSubject->computeRotation(id, rot);
	}

}

void ViconVFileData::getBodyQuaternion(int id, float* qt, bool global)
{
	
	if (id < 0 || id > m_vBodyChannels.size() - 1)
	{
		qt[0] = 1.0f;
		qt[1] = qt[2] = qt[3] = 0.0f;
		return;
	}

	if (global)
	{
		qt[0] = m_vBodyData[m_iCurrFrame][id].QW;
		qt[1] = m_vBodyData[m_iCurrFrame][id].QX;
		qt[2] = m_vBodyData[m_iCurrFrame][id].QY;
		qt[3] = m_vBodyData[m_iCurrFrame][id].QZ;

	}
	else
	{
		qt[0] = m_vBodyData[m_iCurrFrame][id].qW;
		qt[1] = m_vBodyData[m_iCurrFrame][id].qX;
		qt[2] = m_vBodyData[m_iCurrFrame][id].qY;
		qt[3] = m_vBodyData[m_iCurrFrame][id].qZ;

		m_pSubject->computeQuaternion(id, qt);

	}
}

bool ViconVFileData::loadFile(const char* filename)
{
	// open the file
	std::ifstream v(filename, std::ios::binary);
	if (!v) {
		assert("fail to open file");
		return false;
	}

	// read the static portions
	bool result = readHeader(v);
	readSections(v);
	
	// read the dynamic portions
	readDynamics(v);
	
	v.close();

	// rebuild internal data storage (calculate angles)
	reconstructDataSet();

	m_sSource = std::string(filename);

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32	
	int pos = m_sSource.find_last_of("\\");
#else
	int pos = m_sSource.find_last_of("/");
#endif

	m_sSource = m_sSource.substr(pos+1);

	return result;

}

bool ViconVFileData::readHeader(std::ifstream &v) 
{
	bool result = true;

	char buffer[2];
	short version;
	
	v.read(buffer, 2);
	
	if (v.fail()) 
		std::cout << "ERROR: header read failed" << std::endl;
	
	if (buffer[0]!='V') 
		result = false; 
	
	if (buffer[1]!='#') 
		result = false; 
	
	readType(v, &version);

	return result;

}

void ViconVFileData::readSections(std::ifstream &v) 
{
	unsigned int len;
	char buffer_name[28];
	
	while(1) {
		
		// Section length
		readType(v, &len);

		if (len==0) 
		{
			// Reached end of sections
			v.seekg(28, std::ios_base::cur); // skip rest of blank header
			return;
		} 
		else 
		{
			// Section Name
			v.read(buffer_name, 28);

			if (strcmp(buffer_name, "DATAGROUP")==0) 
				readDataGroup(v, len);
			else
				v.seekg(len, std::ios_base::cur);
		}
	}

	return;
}

void ViconVFileData::readDataGroup(std::ifstream &v, int len)
{
	int section_len = 0;
	
	while (section_len < len) 
	{
		// read datagroup
		int just_read = 0;

		// length of DataGroup record (1 short)
		short Reclen = 0;
		readType(v, &Reclen);	
		
		// null terminator of DataGroup?
		if (Reclen==0) 
		{
			section_len += 2;
			//assert(section_len+2==len);
			continue;
		}

		// group id (1 short)
		short groupID;
		readType(v, &groupID);	
		
		// length of desc field (1byte)
		char DL;
		readType(v, &DL);		
		
		// description : Global Bodies | Local Bodies | Markers
		std::string Desc;
		if (DL != 0) 
		{
			// description of datagroup (DL bytes)
			char * buffer_desc = new char[DL];
			v.read(buffer_desc, DL);		
			Desc = std::string(buffer_desc);
			delete [] buffer_desc;
		} 
		else 
		{
			std::cout << "Huh, no desc for datagroup record" << std::endl;
		}
		
		// set group id based on description
		if (Desc=="Markers")
		{
			m_iMarkerGroupID = groupID;
			readMarkerChannels(v);
		}
		else if (Desc=="Global Bodies")
		{
			m_iGlobalBodyGroupID = groupID;
			readBodyChannels(v);
		}
		else if (Desc=="Local Bodies")
		{
			m_iLocalBodyGroupID = groupID;
			readBodyChannels(v, false);
		}

		// add up with lengh (2 bytes)
		just_read = Reclen + 2;

		section_len += just_read;
	}

}


void ViconVFileData::readMarkerChannels(std::ifstream &v)
{
	// storage type for each DOF value (1 byte)
	char type;
	readType(v, &type);

	// width of type (1 byte)
	char width;
	readType(v, &width);	

	// because I don't trust the next generation of computers
	// not to switch datatypes
	if (type==1)
		assert(width==1 && sizeof(char)==1);
	if (type==2)
		assert(width==1 && sizeof(char)==1);
	if (type==3)
		assert(width==2 && sizeof(short)==2);
	if (type==4)
		assert(width==4 && sizeof(int)==4);
	if (type==5)
		assert(width==sizeof(float));
	if (type==6)
		assert(width==sizeof(double));
	if (type==7)
		assert(width==1 && sizeof(char)==1);

	// frame rate (1 float)
	float fr;
	readType(v, &fr);		
	m_fFrameRate = fr;

	// number of DOFs in datagroup ( 1 short)
	short NumDOFS;
	readType(v, &NumDOFS);	
	
	for (int i = 0; i < NumDOFS; ++i) 
	{
		// length of DOFLabel (1 byte)
		char len;
		readType(v, &len);	
		
		// DOFLabel (len bytes)
		char * buffer_DOFlabel = new char[len];
		v.read(buffer_DOFlabel, len);
		std::string label(buffer_DOFlabel);
		delete [] buffer_DOFlabel;

		//	Extract the channel type
		int openBrace = label.find('<');
		
		if(openBrace == label.npos) 
			continue;
	
		int closeBrace = label.find('>');
		
		if(closeBrace == label.npos) 
			continue;

		closeBrace++;

		std::string Type = label.substr(openBrace, closeBrace-openBrace);

		//	Extract the Name
		std::string Name = label.substr(0, openBrace);

		int space = Name.rfind(' ');
		
		if(space != Name.npos) 
			Name.resize(space);

		// traverse marker channel vector...
		std::vector< MarkerChannel >::iterator iMarker;
		std::vector< std::string >::const_iterator iTypes;
		
		iMarker = std::find(	m_vMarkerChannels.begin(), 
								m_vMarkerChannels.end(), Name);

		if(iMarker != m_vMarkerChannels.end())
		{
			//	The channel is for a marker we already have.
			iTypes = std::find(	ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type);
			if(iTypes != ClientCodes::MarkerTokens.end())
			{
				iMarker->operator[](iTypes - ClientCodes::MarkerTokens.begin()) = i;
				m_iNumMarkerChannels++;
			}
		}
		else
		if((iTypes = std::find(ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type))
				!= ClientCodes::MarkerTokens.end())
		{
			//	Its a new marker.
			m_vMarkerChannels.push_back(MarkerChannel(Name));
			m_vMarkerChannels.back()[iTypes - ClientCodes::MarkerTokens.begin()] = i;
			m_iNumMarkerChannels++;
		}
		else
		{
			//	It could be a new channel type.
		}

	}

}

void ViconVFileData::readBodyChannels(std::ifstream &v, bool global)
{
	// storage type for each DOF value (1 byte)
	char type;
	readType(v, &type);

	// width of type (1 byte)
	char width;
	readType(v, &width);	

	// because I don't trust the next generation of computers
	// not to switch datatypes
	if (type==1)
		assert(width==1 && sizeof(char)==1);
	if (type==2)
		assert(width==1 && sizeof(char)==1);
	if (type==3)
		assert(width==2 && sizeof(short)==2);
	if (type==4)
		assert(width==4 && sizeof(int)==4);
	if (type==5)
		assert(width==sizeof(float));
	if (type==6)
		assert(width==sizeof(double));
	if (type==7)
		assert(width==1 && sizeof(char)==1);

	// frame rate (1 float)
	float fr;
	readType(v, &fr);		
	m_fFrameRate = fr;

	// number of DOFs in datagroup ( 1 short)
	short NumDOFS;
	readType(v, &NumDOFS);	
	
	for (int i = 0; i < NumDOFS; ++i) 
	{
		// length of DOFLabel (1 byte)
		char len;
		readType(v, &len);	
		
		// DOFLabel (len bytes)
		char * buffer_DOFlabel = new char[len];
		v.read(buffer_DOFlabel, len);
		std::string label(buffer_DOFlabel);
		delete [] buffer_DOFlabel;

		//	Extract the channel type
		int openBrace = label.find('<');
		
		if(openBrace == label.npos) 
			continue;
	
		int closeBrace = label.find('>');
		
		if(closeBrace == label.npos) 
			continue;

		closeBrace++;

		std::string Type = label.substr(openBrace, closeBrace-openBrace);

		//	Extract the Name
		std::string Name = label.substr(0, openBrace);

		int space = Name.rfind(' ');
		
		if(space != Name.npos) 
			Name.resize(space);

		// traverse marker channel vector...
		std::vector< BodyChannel >::iterator iBody;
		std::vector< std::string >::const_iterator iTypes;
		
		iBody = std::find(	m_vBodyChannels.begin(), 
								m_vBodyChannels.end(), Name);

		if(iBody != m_vBodyChannels.end())
		{
			//	The channel is for a marker we already have.
			iTypes = std::find(	ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type);
			if(iTypes != ClientCodes::BodyTokens.end())
			{
				iBody->operator[](iTypes - ClientCodes::BodyTokens.begin()) = i;
				if (global)
					m_iNumGBodyChannels++;
				else
					m_iNumLBodyChannels++;
			}
		}
		else
		if((iTypes = std::find(ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type))
				!= ClientCodes::BodyTokens.end())
		{
			//	Its a new marker.
			m_vBodyChannels.push_back(BodyChannel(Name));
			m_vBodyChannels.back()[iTypes - ClientCodes::BodyTokens.begin()] = i;
			if (global)
				m_iNumGBodyChannels++;
			else
				m_iNumLBodyChannels++;
		}
		else
		{
			//	It could be a new channel type.
		}

	}

}

void ViconVFileData::readDynamics(std::ifstream &v)
{
	short len;
	short GroupID;
	int frame;
	int prev_frame = -1;
	
	while(1) 
	{
		// Length of data
		readType(v, &len);
		if (v.eof()) 
		{ 
			// reached end of file			
			m_iTotFrames = MAX(gbody_frames.size(), marker_frames.size());
			return; 
		}

		// Group ID
		readType(v, &GroupID);
		
		if (GroupID == m_iGlobalBodyGroupID)
		{
			readType(v, &frame);
			/*
			if (prev_frame != -1 && prev_frame+1 != frame) 
				std::cout << "Frame jump : " << prev_frame << "to " << frame << std::endl;
			*/
			
			prev_frame = frame;
			std::vector< double > d(m_iNumGBodyChannels, 0.0);
			for (unsigned int i = 0; i < d.size(); ++i) 
			{
				readType(v, &(d[i]));
			}
			
			gbody_frames.push_back(d);
		}
		else if (GroupID == m_iLocalBodyGroupID)
		{
			readType(v, &frame);
			/*
			if (prev_frame != -1 && prev_frame+1 != frame) 
				std::cout << "Frame jump : " << prev_frame << "to " << frame << std::endl;
			*/
			
			prev_frame = frame;
			std::vector< double > d(m_iNumLBodyChannels, 0.0);
			for (unsigned int i = 0; i < d.size(); ++i) 
			{
				readType(v, &(d[i]));
			}
			
			lbody_frames.push_back(d);
		}
		else if (GroupID == m_iMarkerGroupID)
		{
			readType(v, &frame);
			/*
			if (prev_frame != -1 && prev_frame+1 != frame) 
				std::cout << "Frame jump : " << prev_frame << "to " << frame << std::endl;
			*/
			
			prev_frame = frame;
			std::vector< double > d(m_iNumMarkerChannels, 0.0);
			for (unsigned int i = 0; i < d.size(); ++i) 
			{
				readType(v, &(d[i]));
			}
			
			marker_frames.push_back(d);
		}
		else
		{
			std::cout << "Ignoring data from different GroupID" << std::endl;
			v.seekg(len - 2, std::ios_base::cur);
		}

	}

}

void ViconVFileData::reconstructDataSet()
{
	// rebuild dataset from raw frame data vector

	// rebuild marker data
	// frames
	if (m_iNumMarkerChannels > 0)
		for (unsigned int i=0; i< m_iTotFrames; i++)
		{
			// channels
			std::vector<MarkerData> m(m_vMarkerChannels.size());
			for (unsigned int j=0; j < m.size(); j++)
			{
				// translation
				m[j].X = -marker_frames[i][m_vMarkerChannels[j].X];
				m[j].Z = marker_frames[i][m_vMarkerChannels[j].Y];
				m[j].Y = marker_frames[i][m_vMarkerChannels[j].Z];
				m[j].Visible = 1;
			}
			m_vMarkerData.push_back(m);
		}

	// rebuild body data
	// frames
	if (m_iNumGBodyChannels > 0)
	{
		for (unsigned int i=0; i< m_iTotFrames; i++)
		{
			// channels
			std::vector<BodyData> b(m_vBodyChannels.size());
			for (unsigned int j=0; j < b.size(); j++)
			{
				// global translation
				if (m_vBodyChannels[j].TX != -1)
				{
					b[j].TX = -gbody_frames[i][m_vBodyChannels[j].TX];
					b[j].TZ = gbody_frames[i][m_vBodyChannels[j].TY];
					b[j].TY = gbody_frames[i][m_vBodyChannels[j].TZ];
				}

				// local translation
				if (m_vBodyChannels[j].tX != -1)
				{
					b[j].tX = -lbody_frames[i][m_vBodyChannels[j].tX];
					b[j].tZ = lbody_frames[i][m_vBodyChannels[j].tY];
					b[j].tY = lbody_frames[i][m_vBodyChannels[j].tZ];
				}

				// rotatiion need some more calculation
				//=============================================================
				//	An angle-axis is vector, the direction of which is the axis
				//	of rotation and the length of which is the amount of 
				//	rotation in radians. 
				//=============================================================
				double len, tmp;
				// global rotation
				if (m_vBodyChannels[j].RX != -1)
				{
					len = sqrt(	gbody_frames[i][m_vBodyChannels[j].RX] * gbody_frames[i][m_vBodyChannels[j].RX] + 
								gbody_frames[i][m_vBodyChannels[j].RY] * gbody_frames[i][m_vBodyChannels[j].RY] + 
								gbody_frames[i][m_vBodyChannels[j].RZ] * gbody_frames[i][m_vBodyChannels[j].RZ] );

					b[j].QW = cos(len / 2.0);
					tmp = sin(len / 2.0);
					if (len < 1e-10) 
					{
						b[j].QX = -gbody_frames[i][m_vBodyChannels[j].RX];
						b[j].QZ = gbody_frames[i][m_vBodyChannels[j].RY];
						b[j].QY = gbody_frames[i][m_vBodyChannels[j].RZ];
					} 
					else 
					{
						b[j].QX = -gbody_frames[i][m_vBodyChannels[j].RX] * tmp/len;
						b[j].QZ = gbody_frames[i][m_vBodyChannels[j].RY] * tmp/len;
						b[j].QY = gbody_frames[i][m_vBodyChannels[j].RZ] * tmp/len;
					}
					
					//	The following converts angle-axis to a rotation matrix.
					double c, s, x, y, z;
					c=s=x=y=z=0;

					if (len < 1e-15)
					{
						b[j].GlobalRotation[0][0] = b[j].GlobalRotation[1][1] = b[j].GlobalRotation[2][2] = 1.0;
						b[j].GlobalRotation[0][1] = b[j].GlobalRotation[0][2] = b[j].GlobalRotation[1][0] =
							b[j].GlobalRotation[1][2] = b[j].GlobalRotation[2][0] = b[j].GlobalRotation[2][1] = 0.0;
					}
					else
					{
						x = -gbody_frames[i][m_vBodyChannels[j].RX]/len;
						z = gbody_frames[i][m_vBodyChannels[j].RY]/len;
						y = gbody_frames[i][m_vBodyChannels[j].RZ]/len;

						c = cos(len);
						s = sin(len);

						b[j].GlobalRotation[0][0] = c + (1-c)*x*x;
						b[j].GlobalRotation[0][1] =     (1-c)*x*y + s*(-z);
						b[j].GlobalRotation[0][2] =     (1-c)*x*z + s*y;
						b[j].GlobalRotation[1][0] =     (1-c)*y*x + s*z;
						b[j].GlobalRotation[1][1] = c + (1-c)*y*y;
						b[j].GlobalRotation[1][2] =     (1-c)*y*z + s*(-x);
						b[j].GlobalRotation[2][0] =     (1-c)*z*x + s*(-y);
						b[j].GlobalRotation[2][1] =     (1-c)*z*y + s*x;
						b[j].GlobalRotation[2][2] = c + (1-c)*z*z;
					}

					// now convert rotation matrix to nasty Euler angles (yuk)
					// you could convert direct from angle-axis to Euler if you wish

					//	'Look out for angle-flips, Paul...'
					//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
					assert(fabs(b[j].GlobalRotation[0][2]) <= 1);
					b[j].EulerY = asin(-b[j].GlobalRotation[2][0]);

					if(fabs(cos(y)) > 
						std::numeric_limits<double>::epsilon() ) 	// cos(y) != 0 Gimbal-Lock
					{
						b[j].EulerX = atan2(b[j].GlobalRotation[2][1], b[j].GlobalRotation[2][2]);
						b[j].EulerZ = atan2(b[j].GlobalRotation[1][0], b[j].GlobalRotation[0][0]);
					}
					else
					{
						b[j].EulerZ = 0;
						b[j].EulerX = atan2(b[j].GlobalRotation[0][1], b[j].GlobalRotation[1][1]);
					}

				}

				// local rotation
				if (m_vBodyChannels[j].rX != -1)
				{
					len = sqrt(	lbody_frames[i][m_vBodyChannels[j].rX] * lbody_frames[i][m_vBodyChannels[j].rX] + 
								lbody_frames[i][m_vBodyChannels[j].rY] * lbody_frames[i][m_vBodyChannels[j].rY] + 
								lbody_frames[i][m_vBodyChannels[j].rZ] * lbody_frames[i][m_vBodyChannels[j].rZ] );

					b[j].qW = cos(len / 2.0);
					tmp = sin(len / 2.0);
					if (len < 1e-10) 
					{
						b[j].qX = -lbody_frames[i][m_vBodyChannels[j].rX];
						b[j].qZ = lbody_frames[i][m_vBodyChannels[j].rY];
						b[j].qY = lbody_frames[i][m_vBodyChannels[j].rZ];
					} 
					else 
					{
						b[j].qX = -lbody_frames[i][m_vBodyChannels[j].rX] * tmp/len;
						b[j].qZ = lbody_frames[i][m_vBodyChannels[j].rY] * tmp/len;
						b[j].qY = lbody_frames[i][m_vBodyChannels[j].rZ] * tmp/len;
					}
					//	The following converts angle-axis to a rotation matrix.
					double c, s, x, y, z;
					c=s=x=y=z=0;

					if (len < 1e-15)
					{
						b[j].LocalRotation[0][0] = b[j].LocalRotation[1][1] = b[j].LocalRotation[2][2] = 1.0;
						b[j].LocalRotation[0][1] = b[j].LocalRotation[0][2] = b[j].LocalRotation[1][0] =
							b[j].LocalRotation[1][2] = b[j].LocalRotation[2][0] = b[j].LocalRotation[2][1] = 0.0;
					}
					else
					{
						x = -lbody_frames[i][m_vBodyChannels[j].rX]/len;
						z = lbody_frames[i][m_vBodyChannels[j].rY]/len;
						y = lbody_frames[i][m_vBodyChannels[j].rZ]/len;

						c = cos(len);
						s = sin(len);

						b[j].LocalRotation[0][0] = c + (1-c)*x*x;
						b[j].LocalRotation[0][1] =     (1-c)*x*y + s*(-z);
						b[j].LocalRotation[0][2] =     (1-c)*x*z + s*y;
						b[j].LocalRotation[1][0] =     (1-c)*y*x + s*z;
						b[j].LocalRotation[1][1] = c + (1-c)*y*y;
						b[j].LocalRotation[1][2] =     (1-c)*y*z + s*(-x);
						b[j].LocalRotation[2][0] =     (1-c)*z*x + s*(-y);
						b[j].LocalRotation[2][1] =     (1-c)*z*y + s*x;
						b[j].LocalRotation[2][2] = c + (1-c)*z*z;
					}

					// now convert rotation matrix to nasty Euler angles (yuk)
					// you could convert direct from angle-axis to Euler if you wish

					//	'Look out for angle-flips, Paul...'
					//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
					assert(fabs(b[j].LocalRotation[0][2]) <= 1);
					b[j].eulerY = asin(-b[j].LocalRotation[2][0]);

					if(fabs(cos(y)) > 
						std::numeric_limits<double>::epsilon() ) 	// cos(y) != 0 Gimbal-Lock
					{
						b[j].eulerX = atan2(b[j].LocalRotation[2][1], b[j].LocalRotation[2][2]);
						b[j].eulerZ = atan2(b[j].LocalRotation[1][0], b[j].LocalRotation[0][0]);
					}
					else
					{
						b[j].eulerZ = 0;
						b[j].eulerX = atan2(b[j].LocalRotation[0][1], b[j].LocalRotation[1][1]);
					}

				}

			}
			m_vBodyData.push_back(b);
		}
	}

	// now clean up raw storage
	gbody_frames.clear();
	lbody_frames.clear();
	marker_frames.clear();

}

